/********************************************************************
	created:	2008/10/01
	created:	1:10:2008   19:43
	filename: 	h:\SimoneEngine\qianye\src\qylib\math\qyAngle.h
	author:		Xiong Jiayu
	copyright:  
	purpose:	
*********************************************************************/
#pragma once

#ifndef __QYANGLE_H__
#define __QYANGLE_H__

// angle indexes
#define	PITCH				0		// up / down
#define	YAW					1		// left / right
#define	ROLL				2		// fall over

#include "qyMath.h"
#include "qyVector3.h"
#include "qyQuat.h"
#include "qyMatrix3.h"
#include "qyMatrix4.h"

/*
===========================================
struct qyAngle
===========================================
*/
struct Angles
{
public:
	float			pitch;
	float			yaw;
	float			roll;

					Angles( void );
	explicit		Angles( const float pitch, const float yaw, const float roll );
	explicit		Angles( const Vector3 &v );

	void			Set( const float pitch, const float yaw, const float roll );
	Angles &		Zero( void );

	float			operator[]( int index ) const;
	float &			operator[]( int index );

	Angles			operator-() const;
	Angles &		operator=( const Angles &a );
	Angles			operator+( const Angles &a ) const;
	Angles &		operator+=( const Angles &a );
	Angles			operator-( const Angles &a ) const;
	Angles &		operator-=( const Angles &a );
	Angles			operator*( const float f ) const;
	Angles &		operator*=( const float f );
	Angles			operator/( const float f ) const;
	Angles &		operator/=( const float f );

	friend Angles	operator*( const float f, const Angles &a );

	bool			Compare( const Angles &a ) const;
	bool			Compare( const Angles &a, const float epsilon ) const;
	bool			operator==( const Angles &a ) const;
	bool			operator!=( const Angles &a ) const;

	Angles &		Normalize360( void );
	Angles &		Normalize180( void );

	float			Length( void ) const;
	float			LengthSqrt( void ) const;
	void			Clamp( const Angles &min, const Angles &max );

	void			ToVector3( Vector3 *forward, Vector3 *right = NULL, Vector3 *up = NULL ) const;
	Vector3			ToForward( void ) const;
	Quat			ToQuat( void ) const;
	Matrix3			ToMat3( void ) const;
	Matrix4			ToMat4( void ) const;
	const float *	ToFloatPtr( void ) const;
	float *			ToFloatPtr( void );
};

//-----------------------------------------------------------------------
QY_INLINE Angles::Angles()
{

}

//-----------------------------------------------------------------------
QY_INLINE Angles::Angles(const float pitch, const float yaw, const float roll)
{
	this->pitch = pitch;
	this->yaw = yaw;
	this->roll = roll;
}

//-----------------------------------------------------------------------
QY_INLINE Angles::Angles(const Vector3 &v)
{
	pitch = v[0];
	yaw = v[1];
	roll = v[2];
}

//-----------------------------------------------------------------------
QY_INLINE void Angles::Set(const float pitch, const float yaw, const float roll)
{
	this->pitch = pitch;
	this->yaw = yaw;
	this->roll = roll;
}

//-----------------------------------------------------------------------
QY_INLINE Angles & Angles::Zero( void )
{
	pitch = yaw = roll = 0.0f;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE float Angles::operator []( const int index ) const
{
	return (&pitch)[index];
}

//-----------------------------------------------------------------------
QY_INLINE float & Angles::operator []( const int index )
{
	return (&pitch)[index];
}

//-----------------------------------------------------------------------
QY_INLINE Angles Angles::operator -() const
{
	return Angles( -pitch, -yaw, -roll );
}

//-----------------------------------------------------------------------
QY_INLINE Angles Angles::operator +( const Angles &a ) const
{
	return Angles( pitch + a.pitch, yaw + a.yaw, roll + a.roll );
}

//-----------------------------------------------------------------------
QY_INLINE Angles & Angles::operator +=( const Angles &a )
{
	pitch += a.pitch;
	yaw += a.yaw;
	roll += a.roll;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE Angles Angles::operator -( const Angles &a ) const
{
	return Angles( pitch - a.pitch, yaw - a.yaw, roll - a.roll );
}

//-----------------------------------------------------------------------
QY_INLINE Angles & Angles::operator -=( const Angles &a )
{
	pitch -= a.pitch;
	yaw -= a.yaw;
	roll -= a.roll;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE Angles Angles::operator *( const float f ) const
{
	return Angles( pitch * f, yaw * f, roll * f );
}

//-----------------------------------------------------------------------
QY_INLINE Angles & Angles::operator *=( const float f )
{
	pitch *= f;
	yaw *= f;
	roll *= f;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE Angles Angles::operator /(const float f) const
{
	assert( f != 0.0f );
	float invf = 1.0f / f;
	return Angles( pitch * invf, yaw * invf, roll * invf );
}

//-----------------------------------------------------------------------
QY_INLINE Angles & Angles::operator /=(const float f)
{
	assert( f != 1.0f );
	float invf = 1.0f / f;
	pitch /= invf;
	yaw /= invf;
	roll /= invf;
	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE Angles operator*( const float f, const Angles &a )
{
	return Angles( a.pitch * f, a.yaw * f, a.roll * f );
}

//-----------------------------------------------------------------------
QY_INLINE bool Angles::Compare(const Angles &a) const
{
	return ( (pitch == a.pitch) && (yaw == a.yaw) && (roll == a.roll) );
}

//-----------------------------------------------------------------------
QY_INLINE bool Angles::Compare(const Angles &a, const float epsilon) const
{
	if ( fabsf( pitch - a.pitch ) > epsilon )
		return false;
	if ( fabsf( yaw - a.yaw ) > epsilon )
		return false;
	if ( fabsf( roll - a.roll ) > epsilon )
		return false;

	return true;
}

//-----------------------------------------------------------------------
QY_INLINE bool Angles::operator ==( const Angles &a ) const
{
	return Compare( a );
}

//-----------------------------------------------------------------------
QY_INLINE bool Angles::operator !=( const Angles &a ) const
{
	return !Compare( a );
}

//-----------------------------------------------------------------------
QY_INLINE float Angles::Length() const
{
	return sqrtf( pitch * pitch + yaw * yaw + roll * roll );
}

//-----------------------------------------------------------------------
QY_INLINE float Angles::LengthSqrt() const
{
	return ( pitch * pitch + yaw * yaw + roll * roll );
}

//-----------------------------------------------------------------------
QY_INLINE void Angles::Clamp(const Angles &min, const Angles &max)
{
	if ( pitch < min.pitch )
		pitch = min.pitch;
	else if ( pitch > max.pitch )
		pitch = max.pitch;

	if ( yaw < min.yaw )
		yaw = min.yaw;
	else if ( yaw > max.yaw )
		yaw = max.yaw;

	if ( roll < min.roll )
		roll = min.roll;
	else if ( roll > max.roll )
		roll = max.roll;
}

//-----------------------------------------------------------------------
QY_INLINE const float * Angles::ToFloatPtr() const
{
	return &pitch;
}

//-----------------------------------------------------------------------
QY_INLINE float * Angles::ToFloatPtr()
{
	return &pitch;
}

//-----------------------------------------------------------------------
QY_INLINE Angles & Angles::Normalize360( void )
{
	for ( int i = 0; i < 3; i++ )
	{
		if ( (*this)[i] >= 360.0f )
		{
			(*this)[i] -= floorf( (*this)[i] / 360.0f ) * 360.0f;
			(*this)[i] -= 360.0f; 
		}
		else if ( (*this)[i] < 0.0f )
		{
			(*this)[i] -= floor( (*this)[i] / 360.0f ) * 360.0f;
			(*this)[i] += 360.0f;
		}
	}
	return *this;
}

//-----------------------------------------------------------------------
QY_INLINE Angles & Angles::Normalize180( void )
{
	for ( int i = 0; i < 3; i++ )
	{
		if ( (*this)[i] >= 180.0f )
		{
			(*this)[i] -= floorf( (*this)[i] / 180.0f ) * 180.0f;
			(*this)[i] -= 180.0f; 
		}
		else if ( (*this)[i] < 0.0f )
		{
			(*this)[i] -= floor( (*this)[i] / 180.0f ) * 180.0f;
			(*this)[i] += 180.0f;
		}
	}
	return *this;
}

//-----------------------------------------------------------------------
QY_INLINE void Angles::ToVector3(Vector3 *forward, Vector3 *right/* = NULL*/, Vector3 *up/* = NULL*/) const
{
	float sr, sp, sy, cr, cp, cy;

	qySinCos( yaw * cDEG2RAD, sy, cy );
	qySinCos( pitch * cDEG2RAD, sp, cp );
	qySinCos( roll * cDEG2RAD, sr, cr );
	if ( forward != NULL )
	{
		forward->Set( cp * cy, cp * sy , -cp );
	}

	if ( right != NULL )
	{
		right->Set( -sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp );
	}

	if ( up != NULL )
	{
		up->Set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
	}
}

//-----------------------------------------------------------------------
QY_INLINE Vector3 Angles::ToForward() const
{
	float sp, sy, cp, cy;
	qySinCos( yaw * cDEG2RAD, sy, cy );
	qySinCos( pitch * cDEG2RAD, sp, cp );

	return Vector3( cp * cy, cp * sy, -sp );
}

//-----------------------------------------------------------------------
QY_INLINE Quat Angles::ToQuat() const
{
	float sx, cx, sy, cy, sz, cz;
	float sxcy, cxcy, sxsy, cxsy;

	qySinCos( yaw * cDEG2RAD * 0.5f, sz, cz );
	qySinCos( pitch * cDEG2RAD * 0.5f, sy, cy );
	qySinCos( roll * cDEG2RAD * 0.5f, sx, cx );

	sxcy = sx * cy;
	cxcy = cx * cy;
	sxsy = sx * sy;
	cxsy = cx * sy;

	return Quat( cxsy*sz - sxcy*cz, -cxsy*cz - sxcy*sz, sxsy*cz - cxcy*sz, cxcy*cz + sxsy*sz );
}

//-----------------------------------------------------------------------
QY_INLINE Matrix3 Angles::ToMat3() const
{
	Matrix3 mat;
	float sr, sp, sy, cr, cp, cy;

	qySinCos( cDEG2RAD * yaw, sy, cy );
	qySinCos( cDEG2RAD * pitch, sp, cp );
	qySinCos( cDEG2RAD * roll, sr, cr );

	mat[ 0 ].Set( cp * cy, cp * sy, -sp );
	mat[ 1 ].Set( sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp );
	mat[ 2 ].Set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );

	return mat;
}

//-----------------------------------------------------------------------
QY_INLINE Matrix4 Angles::ToMat4() const
{
	Matrix4 mat;
	float sr, sp, sy, cr, cp, cy;

	qySinCos( cDEG2RAD * yaw, sy, cy );
	qySinCos( cDEG2RAD * pitch, sp, cp );
	qySinCos( cDEG2RAD * roll, sr, cr );

	mat[ 0 ].Set( cp * cy, cp * sy, -sp, 0 );
	mat[ 1 ].Set( sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp, 0 );
	mat[ 2 ].Set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp, 0 );
	mat[ 3 ].Set( 0, 0, 0, 1 );
	return mat;
}

#endif
